/**
 * @file wheel_control.cpp
 * @author Jae Frank (thissfk@qq.com)
 * @brief
 * @version 0.1
 * @date 2024-01-17
 *
 * @copyright Copyright (c) 2024
 *
 */
#include "wheel_control.hpp"
#include <cmath>

/**
 * @brief Construct a new wheel ctl t::wheel ctl t object构造函数
 *
 * @param robot 要控制webots的Robot类
 * @param motor_name 电机名字数组
 */
wheel_ctl_t::wheel_ctl_t(Robot *robot, const char *motor_name[]) : m_robot(robot)
{
    for (size_t i = 0; i < TTL_WHEEL_CNT; i++)
    {
        m_motor[i] = m_robot->getMotor(motor_name[i]);
        m_motor[i]->setPosition(INFINITY);
        m_motor[i]->setVelocity(0.0);
    }
}

/**
 * @brief 设置电机速度
 *
 * @param index 电机的索引
 * @param speed 设置的速度值
 */
void wheel_ctl_t::set_motor_spd(uint16_t index, spd_t speed)
{
    m_motor[index]->setVelocity(speed);
}

/**
 * @brief Set the 4motors spd object设置四个电机的速度
 *
 * @param speed 要设置的速度
 * @param dir0 方向0
 * @param dir1 方向1
 * @param dir2 方向2
 * @param dir3 方向3
 */
void wheel_ctl_t::set_4motors_spd(spd_t speed, int8_t dir0, int8_t dir1, int8_t dir2, int8_t dir3)
{
    speed = abs(speed);
    set_motor_spd((uint8_t)LF_WHEEL_1, dir0 * speed);
    set_motor_spd((uint8_t)RF_WHEEL_2, dir1 * speed);
    set_motor_spd((uint8_t)LR_WHEEL_3, dir2 * speed);
    set_motor_spd((uint8_t)RR_WHEEL_4, dir3 * speed);
}

/**
 * @brief 前进函数
 *
 * @param speed 前进速度
 */
void wheel_ctl_t::go(spd_t speed)
{
    set_4motors_spd(speed, 1, 1, 1, 1);
}

/**
 * @brief 后退函数
 * 
 * @param speed 后退速度
 */
void wheel_ctl_t::back(spd_t speed)
{
    set_4motors_spd(speed, -1, -1, -1, -1);
}

/**
 * @brief 左转
 * 
 * @param speed 左转速度
 */
void wheel_ctl_t::left(spd_t speed)
{
    set_4motors_spd(speed, -1, 1, -1, 1);
}

/**
 * @brief 右转
 * 
 * @param speed 右转速度
 */
void wheel_ctl_t::right(spd_t speed)
{
    set_4motors_spd(speed, 1, -1, 1, -1);
}

/**
 * @brief 停车
 * 
 */
void wheel_ctl_t::stop(void)
{
    set_4motors_spd(0, 1, 1, 1, 1);
}
